/*!
  @file CameraNodelet.cpp
  @brief Camera driver
  @author Kyohei Otsu
*/

#include <csignal>
#include <cstring>
#include <sstream>

#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <image_transport/camera_subscriber.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <nodelet/nodelet.h>

#include <std_msgs/Int32.h>
#include <sensor_msgs/image_encodings.h>

#include <linux/videodev2.h>
#include <libv4l2.h>
#include "V4L2Camera.cc"

using namespace std;

namespace driver
{

//! @brief Camera driver class
class CameraDriver : public nodelet::Nodelet
{
 private:
	image_transport::Publisher img_pub_[2]; //!< image publisher
  ros::Subscriber req_sub_;             //!< subscriber camera request

  V4L2Camera cam_[2]; //!< V4L2Camera class
  bool cam_on_[2]; //!< true if camera is on

  enum { L=0, R=1, }; //!< camera id

  enum { CAM_W=320, CAM_H=240,}; //!< image size
  
 protected:
  //! @brief callback function for image requests
  //! @param msg Specifies camera id (0: left camera, 1: right camera)
  void requestCb(const std_msgs::Int32ConstPtr &msg);

  //! @brief init V4L2camera
  void initV4L2Camera();

 public:
  //! @brief initialize class
  virtual void onInit();
};

void CameraDriver::onInit()
{
  NODELET_DEBUG("Initializing camera driver..");

  // Get node handler
  ros::NodeHandle &nh = getNodeHandle();
  //ros::NodeHandle &private_nh = getPrivateNodeHandle();
  image_transport::ImageTransport it(nh);

  // Read params

  // register pub/sub
	img_pub_[L] = it.advertise("imageL", 5);
	img_pub_[R] = it.advertise("imageR", 5);
  req_sub_    = nh.subscribe<std_msgs::Int32>("/command/get_camera_image", 5, &CameraDriver::requestCb, this);

  // camera initialization
  initV4L2Camera();
}

void CameraDriver::initV4L2Camera()
{
	cam_[L].connect("/dev/video0");
	if (cam_[L].init(V4L2Camera::SIZE_320_240, 1 )){
		cam_on_[L] = true;
		printf("cam0 on\n");
	}else{ROS_ERROR("cam0 fail\n");}
	
	cam_[R].connect("/dev/video1");	
	if (cam_[R].init(V4L2Camera::SIZE_320_240, 1 )){
		cam_on_[R] = true;
		printf("cam1 on\n");
	}else{ROS_ERROR("cam1 fail\n");}

}

void CameraDriver::requestCb(const std_msgs::Int32ConstPtr &msg)
{
  int id = msg->data;
  //ROS_DEBUG("Get request for camera %d", id);
  if(id !=  L && id != R)
  {
    ROS_ERROR("Invalid camera id %d", id);
    return;
  }
  if(!cam_on_[id])
  {
    ROS_ERROR("Camera not ready %d", id);
    return;
  }

  char *cam_img = cam_[id].getIplImg();
  cv::Mat img(cv::Size(CAM_W, CAM_H), CV_8UC3, cam_img);
  flip(img, img, -1);

  std_msgs::Header head;
  head.stamp = ros::Time::now();
  stringstream ss;
  ss << "/camera" << id;
  head.frame_id = ss.str();
  std::string enc = sensor_msgs::image_encodings::BGR8;
  cv_bridge::CvImage pub_image(head, enc, img);

  img_pub_[id].publish(pub_image.toImageMsg());
}


} // end of namespace

#include <pluginlib/class_list_macros.h>
PLUGINLIB_DECLARE_CLASS(camera, CameraDriver, driver::CameraDriver, nodelet::Nodelet)
